#include "Module_GRBL_13.2.h"
/**
 * 构造函数。初始化模块 I2C 地址
 * @param addr 设备 I2C 地址
 */
Module_GRBL::Module_GRBL(uint8_t addr) {
    this->addr = addr;
}

void Module_GRBL::Init(TwoWire *Wire) {
    _Wire = Wire;
}

void Module_GRBL::Init(TwoWire *Wire, uint32_t x_step, uint32_t y_step,
                       uint32_t z_step, uint32_t acc) {
    _Wire = Wire;
    if (x_step) {
        char code[256];
        sprintf(code, "$0=%d", x_step);
        sendGcode(code);
    }
    if (y_step) {
        char code[256];
        sprintf(code, "$1=%d", y_step);
        sendGcode(code);
    }
    if (z_step) {
        char code[256];
        sprintf(code, "$2=%d", z_step);
        sendGcode(code);
    }
    if (acc) {
        char code[256];
        sprintf(code, "$8=%d", acc);
        sendGcode(code);
    }
}
/******************通信工具函数*********************/
/**
 * 向指定地址写入 G 代码数据
 * @param *c 要写入的数据指针
 */
void Module_GRBL::sendGcode(char *c) {
    _Wire->beginTransmission(addr);
    while ((*c) != 0) {
        _Wire->write(*c);
        c++;
    }
    _Wire->write(0x0d);
    _Wire->write(0x0a);
    _Wire->endTransmission();
}
/**
 * 发送一个原始字节（byte 或 uint8_t）到 GRBL 模块。
 * 用于发送短期间隔命令或特殊控制命令。
 * @param b 要发送的字节
 */
void Module_GRBL::sendByte(byte b) {
    _Wire->beginTransmission(addr);
    _Wire->write(b);
    _Wire->endTransmission();
}
/**
 * 发送多个原始字节（byte 或 uint8_t）到 GRBL 模块。
 * 用于发送短期间隔命令或特殊控制命令。
 * @param data 要发送的字节指针
 * @param size 要发送的字节数
 */
void Module_GRBL::sendBytes(uint8_t *data, size_t size) {
    _Wire->beginTransmission(addr);
    _Wire->write(data, size);
    _Wire->endTransmission();
}
/**
 * 清空设备返回的缓冲区。
 */
void Module_GRBL::readClean() {
    while (1) {
        uint8_t i = 0;
        char data[10];
        _Wire->requestFrom(addr, 10);
        while (_Wire->available() > 0) {
            data[i++] = _Wire->read();
        }
        if (data[9] == 0xff) break;
    }
}
/*
 * 解锁 GRBL。
 * 0x18：ASCII 中的 Ctrl+X，用于发送中断。
 * $X：GRBL 手动解锁命令。
 */
void Module_GRBL::unLock() {
    this->sendByte(0x18);
    delay(5);
    char bytes[] = "$X\r\n";
    this->sendBytes((uint8_t *)bytes, 4);
}
/**
 * 功能：发送直线运动指令（G1）
 */
void Module_GRBL::setMotor(int x, int y, int z, int speed) {
    char code[256];
    memset(code, 0, sizeof(char) * 256);
    sprintf(code, "G1 X%dY%dZ%d F%d", x, y, z, speed);
    return this->sendGcode(code);
}
/**
 * 功能：设置运动模式为相对（G91）或绝对（G90）。
 * G91：相对运动（以当前位置为基准）
 * G90：绝对运动（以原始坐标为基准）
 */
void Module_GRBL::setMode(String mode) {
    if (mode == "distance") {
        char bytes[] = "G91\n";
        this->sendBytes((uint8_t *)bytes, 4);
        this->mode = mode;
    } else if (mode == "absolute") {
        char bytes[] = "G90\n";
        this->sendBytes((uint8_t *)bytes, 4);
        this->mode = mode;
    }
}
/**
 * 功能：等待 GRBL 模块工作完成。
 * 逻辑：通过发送 '@' 查询当前状态；如果返回 'I' 表示设备 idle。
 */
void Module_GRBL::waitIdle() {
    this->readClean();
    while (1) {
        this->sendByte('@');
        char state;
        _Wire->requestFrom(addr, 1);
        if (_Wire->available() > 0) {
            state = _Wire->read();
        }
        if (state == 'I') {
            break;
        }
        delay(5);
    }
}


/**
 * 读取 GRBL 返回信息（例如运行状态、错误等）。
 * 持续读取直到接收到 0xFF。
 */
String Module_GRBL::readLine() {
    String Data = "";
    while (1) {
        uint8_t i = 0;
        char data[10];
        _Wire->requestFrom(addr, 10);
        while (_Wire->available() > 0) {
            data[i] = _Wire->read();
            i++;
        }
        Data += data;
        if (data[9] == 0xff) {
            break;
        }
    }
    return Data;
}
/**
 * 功能：获取 GRBL 当前状态的字符串。
 * 逻辑：先清空缓冲区，发送 @ 命令后读取状态信息。
 */
String Module_GRBL::readStatus() {
    this->readClean();
    this->sendByte('@');
    return this->readLine();
}

/**
 * 判断模块是否为空闲（状态首字符为 'I'）
 */
bool Module_GRBL::readIdle() {
    return this->readStatus()[0] == 'I';
}
/**
 * 判断是否为锁定状态（状态首字符为 'A'）
 */
bool Module_GRBL::inLock() {
    return this->readStatus()[0] == 'A';
}